/*
 * sa2d.cpp
 *
 *  Created on: Aug 2, 2009
 *      Author: andri
 */

#include "sa2d.h"

sa2d::sa2d() {

}

int sa2d::runSim(int argc, const char **argv, configurationfile * configFile){


	/****************************/
	// Reading options form the argument list
	 	int fflag = 0;
		int rflag = 0;
		int pflag = 0;
	    const char *configfile = configFile->getFileName().c_str();
	    int c;
	    opterr = 0;
		while ((c = getopt (argc, (char **)argv, "f:r:p:")) != -1)
		    switch (c)
	        {
	          case 'r': // Percentage of Total time to 	/***********************************/write restart file.
	           rflag = 1;
			   restart_time=atof(optarg);
	          break;
			  case 'p': // Percentage of Total time to write output file.
	           pflag = 1;
			   output_time=atoi(optarg);
	          break;
	          case '?':
					if (optopt == 'f')
	         		fprintf (stderr, "Option -%c requires a filename as argument.\n",
						  optopt);
					else if (optopt == 'r')
	         		fprintf (stderr, "Option -%c requires a real value in (0-1].\n",
						  optopt);
					else if (optopt == 'p')
	         		fprintf (stderr, "Option -%c requires a integer value.\n",
						  optopt);
					else if (isprint (optopt))
	         		fprintf (stderr, "Unknown option `-%c'.\n", optopt);
	       			else
	         		fprintf (stderr,"Unknown option character `\\x%x'.\n", optopt);
			        return 1;
					break;
	           default:
	             abort ();
	       }
	// Some default values
		   if(!rflag)
			   restart_time=0.1;
		   if(!pflag)
			   output_time=100;

	/***********************************/

	// Initialize the simulation
		printf("** Initializing **\n");
		cpInitChipmunk(); // initializing 2D physics engine
		space = cpSpaceNew();
		performer* simPerformer = configFile->getSimulation()->getPerformer();
		physicsEngine* simPhysics = configFile->getSimulation()->getPhysicsEngine();
	    // Performer data
		space->iterations = simPerformer->getMax_substeps();
		dt = simPerformer->getTotal_time()/(simPerformer->getIterations()-1.0);
		
	    /** @todo Add to the performer total_time, timestep_method, max_substeps,
	    	 renderer: scale, fps and window creation.*/

		// Physics engine data
		cpSpaceResizeStaticHash(space, simPhysics->getStatic_shape_hash_dim(), simPhysics->getActive_shape_hash_count());
		cpSpaceResizeActiveHash(space, simPhysics->getActive_shape_hash_dim(), simPhysics->getActive_shape_hash_count());

		space->gravity = cpv(0.0,simPhysics->getGravity());
	    /** @todo gravity should be read from a vector. Comes from a todo in the parser.
	    */
		space->damping = 1.0e0-simPhysics->getDamping();
	    /** @todo Add to the physics engine friction_scaling.*/

	//***********************

	// Initialize the Arena
		staticBody = cpBodyNew(INFINITY, INFINITY);
		printf("Arena:\n");
		initArena(configFile->getArena()); // initialize the Arena
		printf("Arena OK\n\n");

	//***********************

	  // Initialize the Actors
		cpResetShapeIdCounter(); // reset the shape counter

		printf("Actors:\n");
		initActors(configFile); // initialize the actors on the arena
		printf("Actors OK\n\n");
		printf("****\n\n");

	//***********************

	    // Text files with state information

		// Pointer to the bodies
		cpArray* bodies=space->bodies;
//		int num = bodies->num;

		// Kinematic data
		cpFloat *ang,*w;
		cpVect *p,*v;

	  // Pointers to store the state of each actor
		ang=(cpFloat*)malloc(space->bodies->num*sizeof(cpFloat));
		w=(cpFloat*)malloc(space->bodies->num*sizeof(cpFloat));
		p=(cpVect*)malloc(space->bodies->num*sizeof(cpVect));
		v=(cpVect*)malloc(space->bodies->num*sizeof(cpVect));

	  // File with Initial condition *******
		char initfile[255]="";
		FILE* init;
		strcpy(initfile,configfile);
		strcat(initfile,"_kinematics.init"); //File for eventual restart
		printf("Initial state file: %s\n",initfile);
		if( (init = fopen(initfile, "w"))==NULL)
		{
			perror("Initial file failed to open");
			exit (1);
		}
		for(int i=0; i< bodies->num; i++)
		{
			cpBody* actor=(cpBody*)bodies->arr[i];
			p[i]=actor->p; //Position
			v[i]=actor->v; //Speed
			ang[i]=(actor->a);// Rotation angle
			w[i]=actor->w; // Angular velocity
		}
		WriteKinFile(init, space->bodies->num,ang,w,p,v);
		fclose(init);

	 // File for Restart
		char restartfile[255]="";
		strcpy(restartfile,configfile);
		strcat(restartfile,"_kinematics.res"); //File for eventual restart
		printf("Restart file: %s\n",restartfile);
		if( (restart = fopen(restartfile, "w"))==NULL)
		{
			perror("Restart file failed to open");
			exit (1);
		}

		int restart_iter=(int)(restart_time*simPerformer->getIterations());

	 // Files for raw data output
		char outstem[255]="";
		char outputfile[255]="";
		strcpy(outstem,configfile);
		printf("Data of each body in file: %s_bodyX.dat\n",outstem);
		output=(FILE**)malloc(space->bodies->num*sizeof(FILE*));
		for(int i=0; i<space->bodies->num;i++)
		{
			sprintf(outputfile,"%s_body%d.dat",outstem,i);
			if( (output[i] = fopen(outputfile, "w"))==NULL)
			{
				perror("Output file failed to open");
				exit (1);
			}
		}

	//***********************

	  // Main Loop *************
	  // Run the solver
		printf("** Running ... **\n");
		if (!strcmp(simPerformer->getRender()->getName().c_str(),"none"))
		{
	  		while(ticks < simPerformer->getIterations() )
	  		{
				run();
				ticks++;
				if(!(ticks%restart_iter)) //Restart file
				{
					for(int i=0; i< bodies->num; i++)
					{
						cpBody* actor=(cpBody*)bodies->arr[i];
						p[i]=actor->p; //Position
						v[i]=actor->v; //Speed
						ang[i]=(actor->a);// Rotation angle
						w[i]=actor->w; // Angular velocity
					}
					WriteKinFile(restart, space->bodies->num,ang,w,p,v);
					fflush(restart);
					rewind(restart);
				}
			  }
			WriteKinFile(restart, space->bodies->num,ang,w,p,v);
			fclose(restart);
			return 0;
		}
		else
		{
			glutStuff(argc, argv, simPerformer);
			/** @todo Stop the simulation with maximum iterations.*/
		}

	//***********************

//		for(int i=0; i<space->bodies->num;i++)
//			fclose(output[i]);

		return 0;
	}

	//Implementations
	static void	eachBody(cpBody *body, void *data)
	{
		static int i=0;

		// Print kinematic data every printperiod
		if(i==space->bodies->num)
			i=0; //Reset when all bodies have been counted;
		if(!(ticks%output_time))
		{
//			fprintf(output[i],"%d %16.10e %16.10e ",ticks,body->p.x,body->p.y);
//			fprintf(output[i],"%16.10e %16.10e ",body->v.x,body->v.y);
//			fprintf(output[i],"%16.10e %16.10e ",body->f.x,body->f.y);
//			fprintf(output[i],"%16.10e %16.10e ",bo0dy->a,body->w);
//			fprintf(output[i],"%16.10e\n",body->t);
//			fflush(output[i]);
//			i=i+1;
	}
		cpBodyResetForces(body);
	}

	void run()
	{
		static cpArray *bodies = space->bodies;
		//	static cpFloat dt = 1.0e-2; dt is now global :(

		//Reset forces
		cpSpaceEachBody(space, &eachBody, NULL);

		// Long range interaction
		for(int j=0; j<bodies->num; j++)
		{
		  for(int k=j+1; k<bodies->num; k++)
		  {
		     LRangeForceApply((cpBody*)bodies->arr[j],(cpBody*)bodies->arr[k]);
		  }
		}

		// noise
		/** @warning This is a hack to include some noise in the system. */
	/*	static cpVect noise,app=cpvzero;
		static int firstime=1;
		static cpFloat theta=0;
		static cpFloat off[500];

		if(firstime)
		{
			srand(time(NULL));
			firstime=0;
			for (int i=0; i<bodies->num;i++)
				off[i]=(2.0e0*rand()/((double)RAND_MAX)-1.0e0)*1.5708;
		}

	    theta=fmod(theta+2.0*3.1415*5*dt,2*3.1415);*/

		static BehavData bdata;
		for(int j=0; j<bodies->num; j++)
		{
			if(!strcmp(((ActorData*)((cpBody*)bodies->arr[j])->data)->setname,"circle")) //Actor j is in set circle
			{	/*
				noise=cpv(2.0e0*rand()/((double)RAND_MAX)-1.0e0,2.0e0*rand()/((double)RAND_MAX)-1.0e0);
				app=cpv(cos(theta+off[j]),sin(theta+off[j]));
				//	  printf("%g %g %g\n",theta,app.x,app.y);
				cpBodyApplyForce((cpBody*)bodies->arr[j],cpvadd(cpvmult(cpvneg(app),6e0),cpvmult(noise,3e0)),cpvzero);
				*/

				bdata.body=(cpBody*)bodies->arr[j];
//				bdata.param[0]=atof(((ActorData*)bdata.body->data)->behavior->data.params[0].value);
//				bdata.param[1]=atof(((ActorData*)bdata.body->data)->behavior->data.params[1].value);
				((ActorData*)bdata.body->data)->behavior->behav_func[0](&bdata);
			}
		}
		cpSpaceStep(space, dt);
	}


sa2d::~sa2d() {
	// TODO Auto-generated destructor stub
}
